#include "Track.h"
#include "Entity.h"
#include "Point.h"
#include "vector2.h"
#include "Application.h"
#include "GeneticsEngine.h"

static int NUM_NODES;

Track::Track()
: m_hasRaceFinished(true),
m_splineRender(false)
, m_needlePos(0)
{
}

Track::~Track()
{

}

void Track::Init()
{
  // Add the point to the curve.
  m_spline.ctrlpts.push_back(vector2f(100,100));
  m_spline.ctrlpts.push_back(vector2f(100,300));
  m_spline.ctrlpts.push_back(vector2f(300,500));
  m_spline.ctrlpts.push_back(vector2f(400,500));
  m_spline.ctrlpts.push_back(vector2f(500,300));
  m_spline.ctrlpts.push_back(vector2f(500,200));
  m_spline.ctrlpts.push_back(vector2f(400,250));
  m_spline.ctrlpts.push_back(vector2f(300,200));

  // Build the arclength table and normalize.
  m_spline.ArclenTableBuild();
  m_spline.ArclenTableNormalize();

  // Set this spline to be a Catmull-Rom spline.  
  // This spline mode works best as it is closed, and will run through all points.
  m_spline.Mode(Spline2D::CatMullRom);

  //////////////////////////////////////////////////////////////////////////
  // Calculate the section start points along the spline.
  //////////////////////////////////////////////////////////////////////////
  unsigned int max = m_spline.ctrlpts.size()-(m_spline.Step()-1);
  unsigned int size = m_spline.ctrlpts.size();
  vector2f start(0,0);
  unsigned int i;
  float t;

  // Calculate the subsections.
  for(i = 0; i < max; i+=m_spline.Step())
    for(t=0;t<1;t+=0.05f)
    {
      // Calculate the start point for the section.
      start.x = m_spline.InterpCtrlPt(m_spline.ctrlpts[i%size].x, 
        m_spline.ctrlpts[(i+1)%size].x, 
        m_spline.ctrlpts[(i+2)%size].x, 
        m_spline.ctrlpts[(i+3)%size].x, 
        t);
      start.y = m_spline.InterpCtrlPt(m_spline.ctrlpts[i%size].y, 
        m_spline.ctrlpts[(i+1)%size].y, 
        m_spline.ctrlpts[(i+2)%size].y, 
        m_spline.ctrlpts[(i+3)%size].y, 
        t);
      m_subSects.push_back(start);
    }

    NUM_NODES = m_subSects.size();

    // Set the start position for the racers.

    m_startPos = vector2f(m_subSects[0]);

    // Calculate the average section distance.
    m_avgSectionLength = 0;
    for(int i=0; i<NUM_NODES-1; i++)
    {
      vector2f vec = m_subSects[i] - m_subSects[i+1];
      m_avgSectionLength += vec.length();
    }

    m_avgSectionLength /= NUM_NODES;
    m_maxSpeed = m_avgSectionLength/2;

    // Load drawables
    Application& app = Application::GetInstance();

    app.GetResourceManager()->ChangeScript("media\\ImmortalCar.rs");

    m_pTrackBack = app.GetResourceManager()->GetSprite("TrackBack");
    m_pTrackSection = app.GetResourceManager()->GetSprite("TrackSection");
    m_pCarFather = app.GetResourceManager()->GetSprite("FatherCar");
    m_pCarMother = app.GetResourceManager()->GetSprite("MotherCar");

    // Set the car width.
    m_carWidth = 8;

    // Reset the car.
    m_father.currAngle = 0.f;
    float test = m_father.currAngle;
    m_father.currPos = m_father.currVel = vector2f(-100,-100);//set off screen so u cant see it 
    m_father.pEntity = NULL;
    m_father.section = 0;    

    m_hasFinished = false;
}

void Track::Update(float a_dt)
{
  m_delta = a_dt;
}

bool Track::HasRaceFinished() const
{
  return m_hasRaceFinished;
}

void Track::RenderTrack()
{
  if(!Application::GetInstance().GetGeneticsEngine()->IsPaused())
  {
    if(m_splineRender)
      for(int i=0; i<NUM_NODES; i++)
      {
        m_pTrackSection->Render(m_subSects[i].x, m_subSects[i].y);
      }
    else
      m_pTrackBack->Render(0,0);

    m_renderMode = true;
    if(m_renderMode)
    {
      RunLapRender(Application::GetInstance().GetGeneticsEngine()->GetFather());
    }

    // Render the father car at the start for now.
    m_pCarFather->RenderEx(m_father.currPos.x, m_father.currPos.y, m_father.currAngle);
  }
}

void Track::RunLap(Entity* a_pEntity)
{
  //////////////////////////////////////////////////////////////////////////
  // @@ What this method needs to do! @@
  //
  // 1. Put the car back at the start of the track, facing the right direction.
  // 2. Reset the velocity of the car by calculating the right direction,
  //    and multiplying this by the average section length / 2. (Still under
  //    review.)
  // 3. Get the current section of the racer, and look through all the
  //    subsections of that section to find which the racer is closest to. 
  // 3a. @TODO - This may need work, as the racer has no idea whether they are 
  //    at the start or end of a section. Possible fix is if the racer 
  //    subsection > 8, we start to check the subsections of the next section.
  // 4. When we've found the subsection of the track the the player is on,
  //    or should be on, we use it to find the steering vector.
  // 5. Using the distance calculated from the player to the track, we check
  //    against the width of the car to see if the player is in fact on the
  //    track. If not, we apply the 'off-track penalty'.
  // 5a. If the player is in fact off the track, we use the vector previously
  //     calculated to find the closest subsection to the racer as an additional
  //     steering vector, pushing them back towards the track.
  // 6. Finally we use all the steering vectors, speed penalties and current
  //    direction of the player, and we calculate their new position on the track.
  // 7. We add time to the counter to determine how long it's taken to complete
  //    the track, and check for an end game condition.
  //////////////////////////////////////////////////////////////////////////

  // Put the car at the start of the track.
  m_father.currPos = m_startPos;
  m_father.pEntity = a_pEntity;
  m_father.section = 0;

  // Find tne current angle of the track. Test this angle against normalize y for angle
  // of the car. This will be the car's initial direction.
  vector2f currTrackVec = m_subSects[1] - m_subSects[0];
  currTrackVec.normalize();
  m_father.currVel = currTrackVec;

  int i;
  int fitnessVal = 0;
  bool hasFinished = false;
  int offTrackPenalty = Application::GetInstance().GetGeneticsEngine()->GetOffTrackPenalty();

  while(!hasFinished)
  {
    // Find the closest subsection from the car, on the track. Do not search subsections
    // we've already been to. check about 10 subsections, this is how many there should be
    // per section.
    vector2f carToTrack;
    float lastDistance = 1000;
    int startSub = m_father.section; // The starting subsection.
    static int closestSub = startSub; // Used as marker to find closest subsection

    if(closestSub == NUM_NODES-1)
    {
      hasFinished = true;
      m_father.pEntity->SetFitness(fitnessVal);
      closestSub = 0;
      return;
    }

    if(m_father.currPos != m_startPos)
    {
      for(i=closestSub; i<NUM_NODES;i++)
      {
        if(i < NUM_NODES)
        {
          carToTrack = m_subSects[i] - m_father.currPos;
          // If this distance is smaller than the distance to the last sub section,
          // set this subsect as the new closest point.
          float thisDistance = carToTrack.length();
          if(thisDistance < lastDistance)
          {
            lastDistance = carToTrack.length();
            closestSub = i;
            m_father.section = closestSub;
          }
        }
      }
    }

    // We've got the closest subsection. Find a steering vector from it.
    vector2f steer(0,0);
    if(closestSub!=0)
    {
      if(closestSub-1 > 0 && closestSub+1 < NUM_NODES)
      {
        steer = m_subSects[closestSub+1] - m_subSects[closestSub-1];
        steer.normalize();
      }
    }

    // Calculate the new direction for the racer.
    m_father.currVel.normalize();
    m_father.currVel;
    steer;
    m_father.currVel += steer;

    //Check the distance from the track, if it's more than a car width, add it.
    vector2f dist = m_subSects[m_father.section] - m_father.currPos;
    if(dist.length() > m_carWidth)
    {
      if(m_father.section < NUM_NODES)
      {
        m_father.currVel += (carToTrack.normalized());
        m_father.currPos += m_father.currVel * (m_maxSpeed/2) * a_pEntity->GetGenome(m_father.section).m_brakeValue;
      }
    }
    else
    {
      m_father.currPos += ((m_father.currVel * m_maxSpeed) * a_pEntity->GetGenome(m_father.section).m_brakeValue);
    }

    //Increase fitness
    ++fitnessVal;
  }
}
void Track::RunLapRender(Entity* a_pEntity)
{
  //////////////////////////////////////////////////////////////////////////
  // @@ What this method needs to do! @@
  //
  // 1. Put the car back at the start of the track, facing the right direction.
  // 2. Reset the velocity of the car by calculating the right direction,
  //    and multiplying this by the average section length / 2. (Still under
  //    review.)
  // 3. Get the current section of the racer, and look through all the
  //    subsections of that section to find which the racer is closest to. 
  // 3a. @TODO - This may need work, as the racer has no idea whether they are 
  //    at the start or end of a section. Possible fix is if the racer 
  //    subsection > 8, we start to check the subsections of the next section.
  // 4. When we've found the subsection of the track the the player is on,
  //    or should be on, we use it to find the steering vector.
  // 5. Using the distance calculated from the player to the track, we check
  //    against the width of the car to see if the player is in fact on the
  //    track. If not, we apply the 'off-track penalty'.
  // 5a. If the player is in fact off the track, we use the vector previously
  //     calculated to find the closest subsection to the racer as an additional
  //     steering vector, pushing them back towards the track.
  // 6. Finally we use all the steering vectors, speed penalties and current
  //    direction of the player, and we calculate their new position on the track.
  // 7. We add time to the counter to determine how long it's taken to complete
  //    the track, and check for an end game condition.
  //////////////////////////////////////////////////////////////////////////

  if(a_pEntity == NULL)
  {
    return;
  }

  // Put the car at the start of the track.
  static bool firstRun = true;

  if(firstRun)
  {
    m_father.currPos = m_startPos;
    m_father.pEntity = a_pEntity;
    m_father.section = 0;

    // Find tne current angle of the track. Test this angle against normalize y for angle
    // of the car. This will be the car's initial direction.
    vector2f currTrackVec = m_subSects[1] - m_subSects[0];
    currTrackVec.normalize();
    m_father.currVel = currTrackVec;
    m_father.currAngle = atan2(currTrackVec.y,currTrackVec.x) - atan2(1.0f,0.0f);

    firstRun = false;
  }

  int i;
  int offTrackPenalty = Application::GetInstance().GetGeneticsEngine()->GetOffTrackPenalty();

  // Find the closest subsection from the car, on the track. Do not search subsections
  // we've already been to. check about 10 subsections, this is how many there should be
  // per section.
  vector2f carToTrack;
  float lastDistance = 1000;
  int startSub = m_father.section; // The starting subsection.
  static int closestSub = startSub; // Used as marker to find closest subsection

  static int updateCounter = 0;
  updateCounter++;
  if(updateCounter > a_pEntity->GetFitness())
  {
    closestSub = 0;
    firstRun = true;
    m_hasRaceFinished = true;
    updateCounter = 0;
    return;
  }
  if(closestSub == NUM_NODES-1)
  {
    closestSub = 0;
    firstRun = true;
    m_hasRaceFinished = true;
    return;
  }

  if(m_father.currPos != m_startPos)
  {
    for(i=closestSub; i<NUM_NODES;i++)
    {
      if(i < NUM_NODES)
      {
        carToTrack = m_subSects[i] - m_father.currPos;
        // If this distance is smaller than the distance to the last sub section,
        // set this subsect as the new closest point.
        float thisDistance = carToTrack.length();
        if(thisDistance < lastDistance)
        {
          lastDistance = carToTrack.length();
          closestSub = i;
          m_father.section = closestSub;
        }
      }
    }
  }

  m_needlePos = 45+(closestSub*3);

  // We've got the closest subsection. Find a steering vector from it.
  vector2f steer(0,0);
  if(closestSub!=0)
  {
    if(closestSub-1 > 0 && closestSub+1 < NUM_NODES)
    {
      steer = m_subSects[closestSub+1] - m_subSects[closestSub-1];
      steer.normalize();
    }
  }

  // Calculate the new direction for the racer.
  m_father.currVel.normalize();
  m_father.currVel;// *= 100;
  steer;// *= 50;
  m_father.currVel += steer;

  //Check the distance from the track, if it's more than a car width, add it.
  vector2f dist = m_subSects[m_father.section] - m_father.currPos;
  if(dist.length() > m_carWidth)
  {
    if(m_father.section < NUM_NODES)
    {
      m_father.currVel += (carToTrack.normalized());//*30);
      m_father.currPos += (m_father.currVel * (m_maxSpeed/2)) * a_pEntity->GetGenome(m_father.section).m_brakeValue;// * m_delta;
    }
  }
  else
  {
    m_father.currPos += ((m_father.currVel * m_maxSpeed) * a_pEntity->GetGenome(m_father.section).m_brakeValue);// * m_delta;
  }

  m_father.currAngle = atan2(m_father.currVel.y, m_father.currVel.x) - atan2(1.0f,0.0f);
}
void Track::ResetShit()
{
  m_hasRaceFinished = false;
}
